#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_msgs/srv/set_initial_pose.hpp"

using namespace std::chrono_literals;

class RobotPoseInit : public rclcpp::Node
{
private:
    rclcpp::Client<nav2_msgs::srv::SetInitialPose>::SharedPtr setinitialpose_client_;

public:
    RobotPoseInit() : Node("set_initial_pose_node")
    {
        setinitialpose_client_ = this->create_client<nav2_msgs::srv::SetInitialPose>("/set_initial_pose");

        while (!setinitialpose_client_->wait_for_service(5s))
        {
            RCLCPP_ERROR(this->get_logger(), "等待/set_initial_pose服务器启动超时！");
        }
        auto request = std::make_shared<nav2_msgs::srv::SetInitialPose::Request>();
        request->pose.header.frame_id = "map";
        request->pose.header.stamp = this->get_clock()->now();
        request->pose.pose.pose.position.x = 0.0;
        request->pose.pose.pose.position.y = 0.0;
        request->pose.pose.pose.orientation.w = 1.0;
        auto future = setinitialpose_client_->async_send_request(request);

        try
        {
            auto response = rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
            if (response == rclcpp::FutureReturnCode())
            {
                RCLCPP_INFO(this->get_logger(), "初始化位姿成功！");
            }
            else
            {
                RCLCPP_WARN(this->get_logger(),"初始化位姿失败！");
            }
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(),"呼叫服务器失败%s!",e.what());
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<RobotPoseInit>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
